#
#    Copyright (C) 2016 Francisco Javier Parra <franparpe@openmailbox.org>
#
#    This program is free software; you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation; either version 2 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program; if not, write to the Free Software
#    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
#

import math
import numpy
from qtcam.core.parameters import SectionParameters


class DoubleHarmonic(SectionParameters):
    """This class represents the double harmonic section of a cam:
    this section is used when it is necessary to prevent the acceleration
    to be zero when on the highest possible position is reached and it
    is necessary to go downwards inmediately afterwards

    """
    def __init__(self, LowestPosition=0.0,
                 HighestPosition=50.0, StartAngle=0.0,
                 EndAngle=100.0, Precision=0.1, AngularVelocity=1):
        super(DoubleHarmonic, self).__init__(LowestPosition,
                                             HighestPosition,
                                             StartAngle,
                                             EndAngle,
                                             Precision,
                                             AngularVelocity)
        self.abcissaeValues = numpy.arange(self.StartAngle,
                                           2 * self.EndAngle, Precision)

    def Position(self, currentAngle):
        """It returns the current position given the current
        angle as a parameter

        Keyword Arguments:
        self --
        currentAngle --
        """
        theta = currentAngle - self.StartAngle
        beta = self.EndAngle - self.StartAngle
        result = "error"
        if 0 <= theta/beta <= 1:
            result = (self.LowestPosition +
                      (self.HighestPosition / 2) *
                      ((1 - math.cos((math.pi * theta) /
                                     beta)) - 0.25 *
                       (1 - math.cos((2 * math.pi * theta) /
                                     beta))))
        elif 1 < theta/beta:
            theta = 1
            beta = 1
            result = (self.LowestPosition +
                      (self.HighestPosition / 2) *
                      ((1 - math.cos((math.pi * theta) /
                                     beta)) - 0.25 *
                       (1 - math.cos((2 * math.pi * theta) /
                                     beta))))
            theta = currentAngle - (self.StartAngle + self.EndAngle)
            beta = self.EndAngle - self.StartAngle
            result = (result -
                      (self.HighestPosition / 2) *
                      ((1 - math.cos((math.pi * theta) /
                                     beta)) + 0.25 *
                       (1 - math.cos((2 * math.pi * theta) /
                                     beta))))
        else:
            print("error")
        return result

    def Velocity(self, currentAngle):
        """Returns the velocity given an angle as an argument
        Keyword Arguments:
        currentAngle -- 
        """
        theta = currentAngle - self.StartAngle
        beta = self.EndAngle - self.StartAngle
        result = "error"
        if 0 <= theta/beta <= 1:
            result = (self.AngularVelocity *
                      (self.HighestPosition * math.pi / (2 * math.radians(beta))) *
                      ((math.sin((math.pi * theta) /
                                 beta)) - 0.5 *
                       (math.sin((2 * math.pi * theta) /
                                 beta))))
        elif 1 < theta/beta:
            theta = currentAngle - (self.StartAngle + self.EndAngle)
            beta = self.EndAngle - self.StartAngle
            result = -(self.AngularVelocity *
                       (self.HighestPosition * math.pi / (2 * math.radians(beta))) *
                       ((math.sin((math.pi * theta) /
                                  beta)) + 0.5 *
                        (math.sin((2 * math.pi * theta) /
                                  beta))))

        else:
            print("error")
        return result

    def Acceleration(self, currentAngle):
        """Returns the acceleration value
        Keyword Arguments:
        currentAngle --
        """
        theta = currentAngle - self.StartAngle
        beta = self.EndAngle - self.StartAngle
        result = "error"
        if 0 <= theta/beta <= 1:
            result = (math.pow(self.AngularVelocity, 2) *
                      (self.HighestPosition * math.pow(math.pi, 2) /
                       (2 * math.pow(math.radians(beta), 2))) *
                      ((math.cos((math.pi * theta) /
                                 beta)) -
                       (math.cos((2 * math.pi * theta) /
                                 beta))))
        elif 1 < theta/beta:
            theta = currentAngle - (self.StartAngle + self.EndAngle)
            beta = self.EndAngle - self.StartAngle
            result = -(math.pow(self.AngularVelocity, 2) *
                       (self.HighestPosition * math.pow(math.pi, 2) /
                        (2 * math.pow(math.radians(beta), 2))) *
                       ((math.cos((math.pi * theta) /
                                  beta)) +
                        (math.cos((2 * math.pi * theta) /
                                  beta))))
        else:
            print("error")
        return result

    def OverAcceleration(self, currentAngle):
        """Returns the value of the jerks

        Keyword Arguments:
        currentAngle --
        """
        theta = currentAngle - self.StartAngle
        beta = self.EndAngle - self.StartAngle
        result = "error"
        if 0 <= theta/beta <= 1:
            result = -(math.pow(self.AngularVelocity, 3) *
                       (self.HighestPosition * math.pow(math.pi, 3) /
                        (2 * math.pow(math.radians(beta), 3))) *
                       ((math.sin((math.pi * theta) /
                                  beta)) - 2 *
                        (math.sin((2 * math.pi * theta) /
                                  beta))))
        elif 1 < theta/beta:
            theta = currentAngle - (self.StartAngle + self.EndAngle)
            beta = self.EndAngle - self.StartAngle
            result = (math.pow(self.AngularVelocity, 3) *
                      (self.HighestPosition * math.pow(math.pi, 3) /
                       (2 * math.pow(math.radians(beta), 3))) *
                      ((math.sin((math.pi * theta) /
                                 beta)) + 2 *
                       (math.sin((2 * math.pi * theta) /
                                 beta))))
        else:
            print("error")
        return result

    def ArrayPosition(self):
        """Returns an array with all the position values

        """
        self.abcissaeValues = numpy.arange(self.StartAngle, self.EndAngle,
                                           self.Precision)
        theta = (self.abcissaeValues - self.StartAngle)
        beta = self.EndAngle - self.StartAngle

        rise = (self.LowestPosition +
                (self.HighestPosition / 2) *
                ((1 - numpy.cos((numpy.pi * theta) /
                                beta)) - 0.25 *
                 (1 - numpy.cos((2 * numpy.pi * theta) /
                                beta))))
        descent = (self.HighestPosition -
                   (self.HighestPosition / 2) *
                   ((1 - numpy.cos((numpy.pi * theta) /
                                   beta)) + 0.25 *
                    (1 - numpy.cos((2 * numpy.pi * theta) /
                                   beta))))
        self.abcissaeValues = numpy.arange(self.StartAngle, 2 * self.EndAngle,
                                           self.Precision)
        return numpy.concatenate((rise, descent))

    def ArrayVelocity(self):
        """Returns an array with all the velocity values
        
        """
        self.abcissaeValues = numpy.arange(self.StartAngle, self.EndAngle,
                                           self.Precision)
        theta = (self.abcissaeValues - self.StartAngle)
        beta = self.EndAngle - self.StartAngle

        rise = (self.AngularVelocity *
                (self.HighestPosition * numpy.pi / (2 * math.radians(beta))) *
                ((numpy.sin((numpy.pi * theta) /
                            beta)) - 0.5 *
                 (numpy.sin((2 * numpy.pi * theta) /
                            beta))))

        descent = -(self.AngularVelocity *
                    (self.HighestPosition * numpy.pi / (2 * math.radians(beta))) *
                    ((numpy.sin((numpy.pi * theta) /
                                beta)) + 0.5 *
                     (numpy.sin((2 * numpy.pi * theta) /
                                beta))))
        self.abcissaeValues = numpy.arange(self.StartAngle, 2 * self.EndAngle,
                                           self.Precision)

        return numpy.concatenate((rise, descent))

    def ArrayAcceleration(self):
        """Returns and array with all the Acceleration values
        
        """
        self.abcissaeValues = numpy.arange(self.StartAngle, self.EndAngle,
                                           self.Precision)
        theta = (self.abcissaeValues - self.StartAngle)
        beta = self.EndAngle - self.StartAngle

        rise = (numpy.power(self.AngularVelocity, 2) *
                (self.HighestPosition * numpy.power(numpy.pi, 2) /
                 (2 * numpy.power(math.radians(beta), 2))) *
                ((numpy.cos((numpy.pi * theta) / beta)) -
                 (numpy.cos((2 * numpy.pi * theta) /
                            beta))))

        descent = -(numpy.power(self.AngularVelocity, 2) *
                    (self.HighestPosition * numpy.power(math.pi, 2) /
                     (2 * numpy.power(math.radians(beta), 2))) *
                    ((numpy.cos((numpy.pi * theta) / beta)) +
                    (numpy.cos((2 * numpy.pi * theta) /
                               beta))))

        self.abcissaeValues = numpy.arange(self.StartAngle, 2 * self.EndAngle,
                                           self.Precision)

        return numpy.concatenate((rise, descent))

    def ArrayOverAcceleration(self):
        """Returns and array with all the jerks values
        
        """
        self.abcissaeValues = numpy.arange(self.StartAngle, self.EndAngle,
                                           self.Precision)
        theta = (self.abcissaeValues - self.StartAngle)
        beta = self.EndAngle - self.StartAngle

        rise = -(numpy.power(self.AngularVelocity, 3) *
                 (self.HighestPosition * numpy.power(math.pi, 3) /
                  (2 * numpy.power(math.radians(beta), 3))) *
                 ((numpy.sin((numpy.pi * theta) /
                             beta)) - 2 *
                  (numpy.sin((2 * math.pi * theta) /
                             beta))))

        descent = (numpy.power(self.AngularVelocity, 3) *
                   (self.HighestPosition * numpy.power(numpy.pi, 3) /
                   (2 * numpy.power(math.radians(beta), 3))) *
                   ((numpy.sin((numpy.pi * theta) /
                               beta)) + 2 *
                    (numpy.sin((2 * numpy.pi * theta) /
                               beta))))

        self.abcissaeValues = numpy.arange(self.StartAngle, 2 * self.EndAngle,
                                           self.Precision)

        return numpy.concatenate((rise, descent))
